#include <stdio.h>
#include "led.h"
#include "touch_pad.h"


bool hpir1(float x[], float y[], int  n, float a[], int m, float dt[])
{ 
    //the polynomial fit function
    int i, j, k;
    float z, p,cc, g, q, d1, d2, s[20], t[20], b[20];
    for (i = 0; i <= m - 1; i++) a[i] = 0.0;
    if (m > n) m = n;
    if (m > 20) m = 20;
    z = 0.0;
    for (i = 0; i <= n - 1; i++) z = z + x[i] / (1.0 * n);
    b[0] = 1.0; d1 = 1.0 * n; p = 0.0; cc = 0.0;
    for (i = 0; i <= n - 1; i++)
    {
        p = p + (x[i] - z); cc = cc + y[i];
    }    
    cc = cc / d1; p = p / d1;
    a[0] = cc * b[0];

    if (m > 1)
    {
        t[1] = 1.0; t[0] = -p;
        d2 = 0.0; cc = 0.0; g = 0.0;
        for (i = 0; i <= n - 1; i++)
        {
            q = x[i] - z - p; d2 = d2 + q * q;
            cc = cc + y[i] * q;
            g = g + (x[i] - z) * q * q;
        }
        cc = cc / d2; p = g / d2; q = d2 / d1;
        d1 = d2;
        a[1] = cc * t[1]; a[0] = cc * t[0] + a[0];
    }
    for (j = 2; j <= m - 1; j++)
    {
        s[j] = t[j - 1];
        s[j - 1] = -p * t[j - 1] + t[j - 2];
        if (j >= 3)
            for (k = j - 2; k >= 1; k--)
                s[k] = -p * t[k] + t[k - 1] - q * b[k];
        s[0] = -p * t[0] - q * b[0];
        d2 = 0.0; cc = 0.0; g = 0.0;
        for (i = 0; i <= n - 1; i++)
        {
            q = s[j];
            for (k = j - 1; k >= 0; k--)
                q = q * (x[i] - z) + s[k];
            d2 = d2 + q * q; cc = cc + y[i] * q;
            g = g + (x[i] - z) * q * q;
        }
        cc = cc / d2; p = g / d2; q = d2 / d1;
        d1 = d2;
        a[j] = cc * s[j]; t[j] = s[j];
        for (k = j - 1; k >= 0; k--)
        {
            a[k] = cc * s[k] + a[k];
            b[k] = t[k]; t[k] = s[k];
        }
    }
    dt[0] = 0.0; dt[1] = 0.0; dt[2] = 0.0;
    for (i = 0; i <= n - 1; i++)
    {
        q = a[m - 1];
        for (k = m - 2; k >= 0; k--)
            q = a[k] + q * (x[i] - z);
        p = q - y[i];
        if (fabs(p) > dt[2]) dt[2] = fabs(p);
        dt[0] = dt[0] + p * p;
        dt[1] = dt[1] + fabs(p);
    }
return 1;
}

#define RANK_  	3  		多项式次数
/*
*********************************************************************************************************
*   函 数 名: polyfit
*   功能说明: 多项式曲线拟合（与matlab效果一致）
*   形    参: d_X	输入的数据的x值
			  d_Y	输入的数据的y值
			  d_N	输入的数据的个数
			  rank  多项式的次数
			  coeff 输出的系数
*   返 回 值: 无
*********************************************************************************************************
*/
//原理：At * A * C = At * Y	，其中 At 为 A 转置矩阵，C 为系数矩阵
#define RANK_ 2
void polyfit(double *d_X, double *d_Y, int d_N, int rank, double *coeff)
{
	if(RANK_ != rank)	//判断次数是否合法
		return;

	int i,j,k;	
	double aT_A[RANK_ + 1][RANK_ + 1] = {0};
	double aT_Y[RANK_ + 1] = {0};
	
	
	for(i = 0; i < rank + 1; i++)	//行
	{
		for(j = 0; j < rank + 1; j++)	//列
		{
			for(k = 0; k < d_N; k++)	
			{
				aT_A[i][j] +=  pow(d_X[k], i+j);		//At * A 线性矩阵
			}
		}
	}
	
	for(i = 0; i < rank + 1; i++)
	{
		for(k = 0; k < d_N; k++)
		{
			aT_Y[i] += pow(d_X[k], i) * d_Y[k];		//At * Y 线性矩阵
		}
	}
	
	//以下为高斯列主元素消去法解线性方程组
	for(k = 0; k < rank + 1 - 1; k++)
	{
		int row = k;
		double mainElement = fabs(aT_A[k][k]);
		double temp = 0.0;
		
		//找主元素
		for(i = k + 1; i < rank + 1 - 1; i++)
		{
			if( fabs(aT_A[i][i]) > mainElement )
			{
				mainElement = fabs(aT_A[i][i]);
				row = i;
			}
		}
		
		//交换两行
		if(row != k)
		{
			for(i = 0; i < rank + 1; i++)
			{
				temp = aT_A[k][i];
				aT_A[k][i] = aT_A[row][i];
				aT_A[row][i] = temp;
			}	
			temp = aT_Y[k];
			aT_Y[k] = aT_Y[row];
			aT_Y[row] = temp;
		}
			
		
		//消元过程
		for(i = k + 1; i < rank + 1; i++)
		{
			for(j = k + 1; j < rank + 1; j++)
			{
				aT_A[i][j] -= aT_A[k][j] * aT_A[i][k] / aT_A[k][k];
			}
			aT_Y[i] -= aT_Y[k] * aT_A[i][k] / aT_A[k][k];
		}
	}	
		
	//回代过程
	for(i = rank + 1 - 1; i >= 0; coeff[i] /= aT_A[i][i], i--)
	{
		for(j = i + 1, coeff[i] = aT_Y[i]; j < rank + 1; j++)
		{
			coeff[i] -= aT_A[i][j] * coeff[j];
		}
	}

	return;	
}


void app_main(void)
{
    led_init();
    touch_run();
    // led_point_set_rgb(10,255,0,0);
    // led_point_set_rgb(12,0,255,0);
    // led_point_set_rgb(30,0,0,255);
    // led_refresh();
    // led_set_all_hsv(100,100,20);
    // led_set_all_rgb(0x10,00,00);
    // led_refresh();
    while(1)
    {
        vTaskDelay(1000);
    }
}
